#include "VisioneController.h"
#include <libfreenect.hpp>
#include <iostream>
#include <vector>
#include <cmath>
#include <math.h>
#include <pthread.h>
#include <opencv/cv.h>
#include <opencv2/opencv.hpp>
#include <opencv/cxcore.h>
#include <opencv/highgui.h>
using namespace cv;
using namespace std;

VisioneController::VisioneController(){

	rgbMatTmp = Mat(Size(DIM_COLONNE,DIM_RIGHE),CV_8UC3,Scalar(0));
	rgbMat = Mat(Size(DIM_COLONNE_TAGLIATA,DIM_RIGHE_TAGLIATA),CV_8UC3,Scalar(0));
	depthMatTmp = Mat(Size(DIM_COLONNE,DIM_RIGHE),CV_16UC1);
	depthMat = Mat(Size(DIM_COLONNE_TAGLIATA,DIM_RIGHE_TAGLIATA),CV_16UC1);
	hsvMat = new Mat(Size(DIM_COLONNE_TAGLIATA,DIM_RIGHE_TAGLIATA),CV_8UC3,Scalar(0));
	//infPalla=Scalar(0,175,22);
	//supPalla=Scalar(8,255,255);
	//Lettieri: infPalla=Scalar(170,100,40);
	//Lettieri:*/ supPalla=Scalar(180,255,255);
	//Lettieri: infPorta=Scalar(100,50,10);
	//Lettieri: supPorta=Scalar(120,255,255);

	img_palla_filt2 = Mat(Size(DIM_COLONNE_TAGLIATA,DIM_RIGHE_TAGLIATA),CV_8UC1,Scalar(0));
	img_porta_filt2 = Mat(Size(DIM_COLONNE_TAGLIATA,DIM_RIGHE_TAGLIATA),CV_8UC1,Scalar(0));

	filtro_palla = new Filtro();
	filtro_porta = new Filtro();
	porta= new RiconoscimentoPorta();
	palla= new RiconoscimentoPalla();
	img = cvCreateImage(cvSize(DIM_COLONNE_TAGLIATA,DIM_RIGHE_TAGLIATA), IPL_DEPTH_8U, 3);

	device = &freenect.createDevice<MyFreenectDevice>(0);
	device->startVideo();
	device->startDepth();
	device->setDepthFormat(FREENECT_DEPTH_MM);
}

VisioneController::~VisioneController(){
	device->stopDepth();
	device->stopVideo();
}
void VisioneController::elaborateImage(bool flag){
	device->getVideo(rgbMatTmp);
	device->getDepth(depthMatTmp);
	rgbMat = rgbMatTmp(cv::Range(DIM_RIGHE_TAGLIATA,DIM_RIGHE-1), cv::Range(0,DIM_COLONNE_TAGLIATA-1));
	depthMat = depthMatTmp(cv::Range(DIM_RIGHE_TAGLIATA,DIM_RIGHE-1), cv::Range(0, DIM_COLONNE_TAGLIATA-1));

	cvtColor(rgbMat,*hsvMat,CV_BGR2HSV);
	img_palla_filt2=filtro_palla->FiltroImg(*hsvMat,Scalar(MIN_H_PALLA,MIN_S_PALLA,MIN_V_PALLA), Scalar(SUP_H_PALLA,SUP_S_PALLA,SUP_V_PALLA),DIM_RIGHE_TAGLIATA,DIM_COLONNE_TAGLIATA);
	img_porta_filt2=filtro_porta->FiltroImg(*hsvMat,Scalar(MIN_H_PORTA,MIN_S_PORTA,MIN_V_PORTA), Scalar(SUP_H_PORTA,SUP_S_PORTA,SUP_V_PORTA),DIM_RIGHE_TAGLIATA,DIM_COLONNE_TAGLIATA);

	profPalla = NULL;
	palla= new RiconoscimentoPalla();
	centroPalla=palla->riconoscimento_palla(rgbMat,img_palla_filt2);
	if(centroPalla.x!=NULL && centroPalla.y!=NULL){
		profPalla=palla->calcoloProfonditaPalla(rgbMat,depthMat,centroPalla);
		angoloPalla=palla->calcoloAngoloPalla(rgbMat,centroPalla);
	}
	if(palla!=NULL)
		delete palla;

	*img=hsvMat->operator _IplImage();
	memcpy(img->imageData, hsvMat->data, DIM_COLONNE_TAGLIATA * DIM_RIGHE_TAGLIATA * 3 * sizeof(char));
	porta= new RiconoscimentoPorta();
	maxRettangoloPorta=porta->riconoscimento_porta(rgbMat,img_porta_filt2);
	profPorta=NULL;
	if(maxRettangoloPorta.area()!=0){
		cvSetImageROI(img,maxRettangoloPorta);
		rettangoloRoi=cvCreateImage(cvSize(maxRettangoloPorta.width,maxRettangoloPorta.height),img->depth,img->nChannels);
		cvCopy(img,rettangoloRoi,NULL);
		cvResetImageROI(img);
		profPorta=porta->calcoloProfonditaPorta(rgbMat,depthMat,maxRettangoloPorta,rettangoloRoi,flag);
		angoloPorta=porta->calcoloAngoloPorta(rgbMat,Point(maxRettangoloPorta.x+maxRettangoloPorta.width/2,maxRettangoloPorta.y+maxRettangoloPorta.height/2));
		if(rettangoloRoi!=NULL)
			cvReleaseImage(&rettangoloRoi);
	}
	if(porta!=NULL)
		delete porta;

	imshow("img_tagliata",rgbMat);

	cvWaitKey(5);
}
unsigned short VisioneController::getProfonditaPorta(){
	return profPorta;
}
int VisioneController::getAngoloPorta(){
	return angoloPorta;
}
unsigned short VisioneController::getProfonditaPalla(){
	return profPalla;
}
int VisioneController::getAngoloPalla(){
	return angoloPalla;
}
void VisioneController::alza(){
	device->setTiltDegrees(5);
}
void VisioneController::abbassa(){
	device->setTiltDegrees(-5);
}
void VisioneController::azzera(){
	device->setTiltDegrees(0);
}
void VisioneController::finale(){
	device->setTiltDegrees(3);
}
